#include "main.h"
#include "Hardware_Motor.h"
#include "Software_Chassis.h"
#include <stdlib.h>
#include <math.h>

Chassis_t Chassis;
uint16_t SpeedLevel_value[] = {CHASSIS_SPEEDLEVEL_1, CHASSIS_SPEEDLEVEL_2, CHASSIS_SPEEDLEVEL_3, CHASSIS_SPEEDLEVEL_4, CHASSIS_SPEEDLEVEL_5};

/* 内部函数 *********************/
double Chassis_max(double a, double b, double c, double d)
{
    double max_value = a;
    if (b > max_value) max_value = b;
    if (c > max_value) max_value = c;
    if (d > max_value) max_value = d;
    return max_value;
}
/********************* 内部函数 */

void Chassis_Init(void)
{
    Chassis.LF_Wheel = Motor_GetMotorPtr_LF();
    Chassis.RF_Wheel = Motor_GetMotorPtr_RF();
    Chassis.LB_Wheel = Motor_GetMotorPtr_LB();
    Chassis.RB_Wheel = Motor_GetMotorPtr_RB();
    Motor_Init();
    Motor_Update();
}

void Chassis_Move(int8_t vx, int8_t vy, int8_t turn, uint8_t SpeedLevel)
{
    int8_t LF_v, RF_v, LB_v, RB_v;
    uint32_t LF_output, RF_output, LB_output, RB_output;
    int8_t max_v;

    if(SpeedLevel > 4) SpeedLevel = 4; // 限制速度等级在0-4之间

    LF_v = ( vx + vy + turn);
    RF_v = (-vx + vy - turn);
    LB_v = (-vx + vy + turn);
    RB_v = ( vx + vy - turn);

    if(LF_v > 0)Motor_SetDriction(Chassis.LF_Wheel, 1);
    else if(LF_v < 0)Motor_SetDriction(Chassis.LF_Wheel, -1);
    else Motor_SetDriction(Chassis.LF_Wheel, 0);
    if(RF_v > 0)Motor_SetDriction(Chassis.RF_Wheel, 1);
    else if(RF_v < 0)Motor_SetDriction(Chassis.RF_Wheel, -1);
    else Motor_SetDriction(Chassis.RF_Wheel, 0);
    if(LB_v > 0)Motor_SetDriction(Chassis.LB_Wheel, 1);
    else if(LB_v < 0)Motor_SetDriction(Chassis.LB_Wheel, -1);
    else Motor_SetDriction(Chassis.LB_Wheel, 0);
    if(RB_v > 0)Motor_SetDriction(Chassis.RB_Wheel, 1);
    else if(RB_v < 0)Motor_SetDriction(Chassis.RB_Wheel, -1);
    else Motor_SetDriction(Chassis.RB_Wheel, 0);

    max_v = Chassis_max(abs(LF_v), abs(RF_v), abs(LB_v), abs(RB_v));

    LF_output = (uint32_t)fabs((LF_v * 1.0 / max_v) * SpeedLevel_value[SpeedLevel]);
    RF_output = (uint32_t)fabs((RF_v * 1.0 / max_v) * SpeedLevel_value[SpeedLevel]);
    LB_output = (uint32_t)fabs((LB_v * 1.0 / max_v) * SpeedLevel_value[SpeedLevel]);
    RB_output = (uint32_t)fabs((RB_v * 1.0 / max_v) * SpeedLevel_value[SpeedLevel]);

    // 设置电机速度
    Motor_SetPWM(Chassis.LF_Wheel, LF_output);
    Motor_SetPWM(Chassis.RF_Wheel, RF_output);
    Motor_SetPWM(Chassis.LB_Wheel, LB_output);
    Motor_SetPWM(Chassis.RB_Wheel, RB_output);
}
